import scc32
import varm
import cynematic
import time


s = scc32.SCC32( port = "/dev/tty.usbserial",
                 simulation = True)

c = cynematic.Cynematic( 64, 50, 154, 154, 100 )

a = varm.Arm()

a.cbase(1500)
a.cshoulder(1500)
a.celbow(1500)
a.cwrist(1500)
a.cgrip(1500)

s.set_action( 0, a.cbase )
s.set_action( 1, a.cshoulder )
s.set_action( 2, a.celbow )
s.set_action( 3, a.cwrist )
s.set_action( 4, a.cwristrotate )
s.set_action( 5, a.cgrip )

# a.shoulder(120)
def angle2pos( angle ) :
        return 1500 + angle * 1000.0 / 90

def goto( x,y,z,wrist=0,delay=1000 ) :
    try :
        base, shoulder, elbow, wrist =  c.xyz_to_angles(x,y,z,wrist)
    except ValueError :
        return
    s.send( {0 : angle2pos( base ),
             1 : angle2pos(shoulder),
             2 : angle2pos(elbow),
             3 : angle2pos(wrist ),
             4 : angle2pos(-base)
             },
             delay 
             )
    
# auto = False
auto = True 

goto(0,0,300, delay = 1000)
goto(0,0,200, delay = 1000)
goto(0,0,100, delay = 1000)
goto(0,0,200, delay = 1000)
goto(0,0,300, delay = 1000)
        
try: 
    while True :
        goto(115, -110, 50, delay = 2000 )
    
        goto(115, -110, 20, delay = 1000 )

        a.cgrip(10)

        goto(115, -110, 50, delay = 1000 )

        goto(118, 88, 50, delay = 2000 )

        goto(118, 88, 20, delay = 1000 )

        a.cgrip(30)

        goto(118, 88, 50, delay = 1000 )

        goto(254,0,218,0,delay = 2000)

        if auto : 
            time.sleep(1)
        else :
            input()

        goto(118, 88, 50, delay = 1000 )

        goto(118, 88, 20, delay = 1000 )

        a.cgrip(10)

        goto(118, 88, 50, delay = 1000 )

        goto(115, -110, 50, delay = 2000 )

        goto(115, -110, 20, delay = 1000 )

        a.cgrip(30)

        goto(115, -110, 50, delay = 1000 )

        goto(254,0,218,0,delay = 2000)

        if auto : 
            time.sleep(1)
        else :
            input()

        
finally :
    s.close()
